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I. INTRODUCTION 


The advent of GPS has afforded the aerospace controls engineer a powerful new 
means of controlling air vehicles. Current guidance schemes rely in some part on 
ground based radars, navigational aides, beacons, localizer beams, etc. Guidance 
and control of the air vehicle have been developed seperately and then combined in 
a somewhat adhoc process. Precise tracking of inertial fixed trajectories using an 
onboard GPS integrated GNC suite affords a quantum leap in autonomous flight 
capabilities. The greatest impact of the proposed technology is expected in the area 
of trajectory tracking control for autonomous unmanned vehicles, and automatic 
approach and landing of manned vehicles dicussed next. 

Control of the commercial air traffic throughout the country continues to become 
more and more demanding as an increased number of vehicles vie for limited access 
to the major commercial aviation hubs. Sophisticated and expensive ground based 
radar control facilities employ a large number of personnel to individually instruct 
the pilots of these aircraft on the trajectories they are to fly. Precise control of the 
aircraft trajectory such as required by an approach into an airfield requires constant 
attention from a ground based air traffic controller. Furthermore, ATC ability to 
effectively control the aircraft is influenced by ground based radar coverage and navaid 
equipment available and is often negatively influenced by atmospheric conditions. 
Airfields with limited resources are often unable to take-off and land aircraft requiring 
instrument departures and arrivals. 

Flight patterns around major aviation hubs are, in general, inertial based tra- 
jectories. That is, an aircraft is required to track a certain path over the ground while 


adhering to a certian altitude schedule, irrespective of air mass disturbances. In some 


] 


cases, such as on final approach to land or when the aerodrome is situated among 
significant terrain or cultural development, precise adherence to the desired trajectory 
is crucial for flight safety. In any case, commanding an inertial trajectory directly 
utilizing a GPS integrated GNC system could prove to be more cost effective and 
accurate than current methods. Furthermore, such a guidance scheme could open up 
many more airports to significant commercial air traffic without requiring the capital 
investment and maintenance of ground based radar facilities. 

Unmanned air vehicles can be a cost effective means of power projection. Addi- 
tionally, in some cases human physiological limits may prove to be the limiting factor 
in the performance of an air vehicle. The precision delivery of munitions becomes 
of paramount importance as weapons and weapon delivery platforms continue to in- 
crease in cost, thus limiting their numbers. All of these concerns can be addressed 
by autonomous air vehicles utilizing a GPS aided guidance, navigation and control 
suite. 

All of these applications have a common thread running through them. While 
the particulars of the vehicles may vary considerably, the intent is to acheive au- 
tonomous control of their trajectory. As a proof of concept, this work presents a new 
design process for the synthesis of a guidance, navigation, and control system for a 
UAV named Bluebird. The function of the this GNC system is to track inertial tra- 
jectories. Bluebird is a UAV operated at the Unmanned Air Vehicle Lab at the Naval 
Postgraduate School. It has a 12.5 foot wingspan and a 20 pound payload capability, 
and is currently being equipped with a full avionics suite, including IMU, GPS, and 
air data sensors. 

The design process began with the development of a nonlinear dynamic model of 
Bluebird implemented in SIMULINK. A typical cruise flight condition was chosen as 


the point for linearization. After linearization of the nonlinear model, the work cen- 











tered on the design of a linear controller. LQR (Linear Quadratic Regulator) synthesis 
approach was used since it provides an intuitive means of synthesizing a multivariable 
controller within the framework of real world design constraints. Following the de- 
sign of the LQR controller, the challenge of implementing the linear controller on the 
nonlinear plant was addressed. A novel method of converting commands and outputs 
from inertial to body reference cooridinates was used (Ref. 10]. This method achieves 
automatic recruiting of the actuators, while preserving a certain linearization prop- 
erty. Next, the accuracy of the nonlinear simulation was enhanced with the addition 
of high fidelity models of sensors used onboard Bluebird. Additionally, Kalman filters 
were designed in order to provide optimal state estimates. Finally, the performance 


of the controller was evaluated in simulations with the full nonlinear model. 


Il. DEVELOPMENT OF THE DYNAMIC 
MODEL 


The development of an integrated guidance, navigation, and control system 
required a high fidelity nonlinear model of the aircraft dynamics. The discussion 
begins with an explanation of nomenclature, abbreviations, and a definition of frames 


of reference. 


A. REFERENCE FRAMES 


Three different reference frames are used in this report. They are: 
e Local Tangent Plane or Inertial Reference Frame 
e Body-Fixed Reference Frame 
e Wind or Flight Path Reference Frame 


1. Local Tangent Plane Reference Frame 

The position of the air vehicle must be maintained with respect to the local 
tangent plane coordinate system. This coordinate system is formed by extending a 
ray from the center of the earth to its surface. A plane is attached tangent to the point 
of intersection of the ray with the Earth’s surface. While it is somewhat arbitrary, for 
our purposes here it will be convenient to define the positive x direction as pointing 
east, the positive y direction as pointing north, and the positive z direction as pointing 
up. This is depicted in Figure 2.1. 

For the purposes of this development, the rotation of the earth and its 


associated Coriolis’ forces can be ignored and the local tangent plane reference frame 
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Figure 2.1: Local Tangent Plane Coordinate System 


can be considered to be an inertial reference frame. In this work, {J} is used to 
represent the inertial reference frame. 
2. Body-Fixed Reference Frame 

The body-fixed reference frame is a right hand orthogonal system with 
the origin at the center of gravity of the air vehicle. The positive x direction points 
towards the nose. The positive y direction points out the right wing and the positive z 
direction points towards the bottom of the air vehicle. The velocity of the air vehicle 
with respect to the inertial reference frame, resolved along the x, y, and z axis of the 
body-fixed reference frame, are termed u, v, and w, respectively. The angular rate of 
rotation of the air vehicle with respect to the inertial reference frame, resolved in the 
body-fixed reference frame, are called p, g, and r, respectively. Positive values for the 
forces, moments, angular rates, and linear velocities in the body-fixed reference frame 
are shown in Figure 2.2. The abrieviation, {B}, is used to represent the body-fixed 


reference frame. 
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Figure 2.2: Body-Fixed Coordinate System (Ref. 11] 


3. Fhght Path or Wind Reference Frame 
The wind reference frame is also a right hand orthogonal system with its 
origin at the center of gravity, c.g., of the air vehicle. The x axis is aligned with the 
velocity vector of the air vehicle. The orientation of the wind reference frame with 
respect to the body-fixed reference frame is defined in terms of the angles a and (. 


The equations for a and # are given below. 


a = tan7'(w/u) (2719) 


and 


B = sin='(v/V) (22) 


where the vectors u,v, w, and V are velocity components of the air vehicle defined in 


Figure 2.3. The abrieviation, {W}, is used to represent the wind reference frame. 
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Figure 2.3: Wind or Flight Path Reference Frame (Ref. 11] 


B. COORDINATE TRANSFORMATIONS 

In order to use these three coordinate systems, one must be able to transform 
between them freely. The Euler angles, $,9, and Y, termed roll, pitch, and yaw, 
are defined in order to express the orientation of the body-fixed reference frame with 
respect to the inertial reference frame. For the purposes of this development, a 3-2-1 
Kuler angle transformation will suffice as its singularity occurs at O equal to 90 de- 
grees. The 3-2-1 transformation is given without explanation but a good development 
of Euler angle transformations in general can be found in [Ref. 6]. The nature of 


the angular rotation is more apparent when the transformation is expressed as the 


product of three rotation matricies. In the case of a 3-2-1 rotation sequence, the three 
matrices in Equation 2.3 correspond to rotations about the yaw, pitch, and roll axes 
of the air vehicle. Of course, the three matrices can be multiplied out for an analytic 
result contained in a single matrix , although the resulting matrix is somewhat busy 
to inspect. In any case, the transformation between a free vector resolved in the in- 
ertial reference frame and the same vector resolved in the body-fixed reference frame 


is given by: 


cosW sin 0 cos9 0 —sin9 1 0 0 
‘vy =| —sinU cos¥ 0 i se 0 cos@ sin® |7V (2.3) 
0 0 1 sn® 0 cos9 0 —sin® cos® 


where ‘V is a free vector resolved in {I} and ?V is the same vector resolved in {B}. 
The inverse is also defined. Conveniently, since the transformation is orthonormal, 
the inverse is simply the transpose of the rotation matrices shown in Equation 2.3. 
Not all, transformations, however, are orthonormal. Of particular interest is 
the case of angular rotation rates. The body-fixed reference frame’s angular rate of 
rotation with respect to the inertial reference frame can be related to the rate of 
change of the Euler angles by a transformation matrix. The development is straight 
forward and is fully explained in [Ref. 11]. The final transformation matrix from p, 


q, r to the time rate of change of the Euler angles, 6,0, &, is given by: 


® 1 sin®tanO cos® tanO Pp 
Or — a0 cos ® —sin ® q|. (2.4) 
NY 0 sin®secO cos® secO r 


By integrating Equation 2.4, the time history of the Euler angles can be obtained. 
Aerodynamic forces and moments are often calculated using stability and control 
derivatives defined with respect to the wind reference frame. The angles, a and /, 


define the orientation of the wind reference frame to the body-fixed reference frame. 


Therefore, a transformation matrix can be obtained that relates a free vector, such as 
lift or drag, resolved in {W} to the same vector resolved in {B}. The transformation 


is expressed as: 


cosacos$ —cosasinf —sina 
BV = sin 3 cos 8 0 vay) (2.5) 
sinacos§ —sinasinf cosa 


where ?V is a free vector resolved in {B} and “V is the same vector resolved in 


{W}. 


C. NOTATION 


Some standardized abbreviations will simplify the development of the nonlinear 
kinematic model of the air vehicle. This short-hand is used in the field of robotics 


where multiple frames of reference are common [Ref. 5]. 


‘P., represents the position vector from the origin of the local tangent plane to 


the center of gravity of the air vehicle. 


By, and ®a,, represent the velocity and acceleration, measured at the center of 


gravity of the air vehicle, with respect to {J}, resolved in {B}. The components 


of 7y,, are commonly termed u, v, and w. 


e ‘vy. and 4a., represent the velocity and acceleration, measured at the center of 


gravity of the air vehicle, with respect to {7}, resolved in {J}. 


e 2we is the angular velocity of the {B} coordinate system with respect to {J}, 


resolved in {B}. The components of ?wg are commonly termed p, gq, and r. 


e ‘wg represents the angular velocity of the {B} coordinate system with respect 


to {I}, resolved in {J}. 


e 4 represents the transformation matrix used to express a free vector resolved 


in {B}, in {I}. The inverse is represented by ?R 


e 2 R represents the transformation matrix used to express a free vector resolved 


in {W}, in {B}. The inverse is represented by 9 R. 


e ®F and #N denote the total external inertial force and moment acting on the 


body resolved in {B}. 


e 'F and ‘N denote the total external inertial force and moment acting on the 


body resolved in {J}. 
e ’F, is the inertial angular momentum of the body resolved in {B}. 
e ‘Lis the inertial angular momentum of the body resolved in {J}. 


e Given a vector v, its derivative with respect to {B} is denoted as £(v) 


and its derivative with respect to {I} is denoted as (v) 


D. RIGID BODY EQUATIONS OF MOTION 

In general, an avionics suite on a modern air vehicle utilizes a strapdown IMU. 
A strapdown IMU, as the name implies, maintains a constant orientation in the body- 
fixed reference frame. The output of the sensors on the IMU are resolved in {B}. 
Therefore, among other reasons, it is most convenient to develop the equations of 
motion of the air vehicle in the body-fixed reference frame. 

1. Linear Motion 

An application of Newton’s Law to linear motion of a body states that 

the total external force applied to a body is equal to the mass of the body times its 


inertial acceleration. This could be written in the inertial reference frame as: 


10 


'F = ma, 
where 
i (2.6) 


or in the body-fixed reference frame as follows: 


Bre— m¥qa 


m 7? Ric 
= M “tc. (2.7) 


Coriolis’ theorem can be used to relate the inertial and body accelerations of the air 


vehicle as follows: 


d 
"Seg = GZ ves + Pwp x hn, (2.8) 


where the difference in the derivatives is explained in Chapter IJ, part C. Equation 2.8 
can be substitued into Equation 2.7 in order to obtain the desired expression for the 


sum of the external inertial forces resolved in the body-fixed reference frame. 


d 
= ae mr (78g + “WE ua hy) 


m Lo +m (Pwg x ?u<9). (2.9) 


dt 
2. Angular Rotation 
Euler’s law for the conservation of angular momentum at the center of 


gravity states that: 


Pl 


of ee (2.10) 


where /L. is the angular momentum of the air vehicle with respect to {I} and /N,, 
is the total moment applied to the air vehicle. Equation 2.10 can be written in the 


body-fixed reference frame as: 


SL =p Rese (2.11) 


Coriolis’ theorem can be used again to expand 71, obtaining: 


BL = SP Lig a Re (2.12) 


It can be shown that the angular momentum, ?L,,, of the air vehicle is the product 
of an inertia tensor, defined as Jg, and the body’s angular velocity, ?wg, where we 
ignored all spining elements. Substituting this definition of ?L,, into Equation 2.12, 


results in: 


E Tig = 5 (dep) + Fw x Jp wp. (2.13) 


Recall that aN = PR'N., =" N.,. Using this relationship, Equation 2.13 can be 


equivalently expressed as: 


iN: = 5 (Jaws) + Bwp x Jp wp (2.14) 


3. External Forces and Moments 
Equation 2.9 and Equation 2.14 from the preceding sections can be com- 


pactly expressed as follows. 


12 


=))e m £3y., +m (Bug x Bru) 
— dt, 9 B cg 
| BN | | Jp owe + Bip x Jp? wp | (20) 


By bringing derivative terms in Equation 2.15 to the left hand side, we obtain, 


d aes —Fy, x By, sts ar 
dt | YwB | - | —J5! Fwy x In®we + Jy! ®N | (2.16) 


Next, the forces and moments in Equation 2.16 can be expanded as follows: 


| ae a | _ ° FeraviTaTIonaL + ® Fproputsive + ® FagropyNAMIc | (2.17) 


nN ? NproputsivE + ® NazRODYNAMIC 
where 
FGRAVITATIONAL = force due to gravity 
BF ROPULSIVE = force due to engine's thrust 
BE uERODYNAMIC = force generated by aerodynamic sur f aces 
BNpROPULSIVE = moment generated by engine's thrust 
BNarERODYNAMIC = moment generated by aerodynamic sur faces 


The gravitational force expressed in {J} is given by: 


0 


'Foravity =| 0 
mg 


where ”g” is the gravitational constant. Then 
3 Foravity = 7 R' Foraviry- (2.18) 


The expansion of the propulsive forces and moments is simplified by consid- 
ering the case of centerline thrust. In that case, no external moments are generated, 


i.e., >Nproputsive = 0, and the propulsive forces can be expressed as: 


’Fprop =| 0 |, (2.19) 


where 7’ represents the thrust of the powerplant. 

Aerodynamic forces and moments are commonly calculated using nondi- 
mensional stability and control derivatives. These derivatives are obtained by approx- 
imating the aerodynamic forces and moments acting on the air vehicle using a Taylor 
series expansion about a given trim point. Typically, values for these derivatives are 
available for the first order terms of the expansion only. Sometimes, a few second 
order terms are available, such as the terms associated with a and B (Ref. 7]. All 
other higher order terms are usually ignored in this approximation. 

In general, the aerodynamic forces and moments acting on the air vehicle 
are computed as follows. The nondimensional stability and control derivatives are 
dimensionalized by multiplication by the appropriate constants, such as wing span, 
dynamic pressure, chord, etc. The dimensional derivative is then multiplied by the 
perturbations of each aerodynamic variable or control deflection from its nominal 
trim point. The summation of the forces and moments due to all of the aerodynamic 
variables and control deflections, in addition to the trim value of the forces and 
moments, results in the total aerodynamic force and moment acting on the air vehicle. 

4. The State Space Representation 

In order to implement the dynamic model in a state-space form suitable for 
numerical simulation, the states of the model need to be chosen. This is somewhat 
arbitrary and many choices will work so long as consistency is maintained in the 
approach. 

As was evident from the development of the rigid body equations of motion, 
the body-fixed reference frame is the most convenient coordinate system in which to 
define the states. The first three states are defined as the inertial velocity of the air 


vehicle resolved in the body-fixed reference frame. These are abrieviated as u, v, and 


14 





2 ——— 











w or more compactly as ¥v,... The fourth through sixth states are defined as the 
angular velocity of the air vehicle with respect to {IJ} resolved in {B}. These are 
abrieviated as p, qg, and ror more compactly as wa. 


Control inputs are represented by the vector A: 
[X= |(Oon orca | (2.20) 


where 6., 6,, and 6, are the elevator, rudder, and aileron inputs, respectively. 
Control inputs for the throttle are represented by 5¢,+. 

Typically, the terms in the Taylor series expansion for the aerodynamic 
stability derivatives are partial derivatives with respect to u/U, a, §, p, q, and r, 
where U is the magnitude of the air vehicle’s velocity vector and a and # are the 
angles defining the orientation of {B} with respect to {W} [Ref. 3]. The last three 
variables, p, q, and r, are states of the model. The first three can be represented as 
a combination of states in the model as follows. First note that, 

U 


0| = YR p,,, (220) 
0 


and for small values of angles a and (, a is approximately equal to w/U and £ is 
approximately equal to v/U. 
It turns out, the stability derivatives can be placed in matrix form as 


follows: 


Gem Cree cy. Cp. Cp. Cp.” Co, 


Br i AC icy) a eC CS 
Cm CCC c.. 
Cu, Cu Cn Cn Cn, Cn, 


Similarily, for the control derivatives: 
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oC Co, mCnwC ps 
OA Cis, Cis, Cis, | 


where z’ is the vector composed of u/U, a, 8, p, q, and r. Now the aerodynamic 


forces and moments can be expressed as follows: 


wR 0 aC OC dC 
W / | 
| 0 BR |{Cr +a Mitt a Met FY Naw k (2:22) 


where M’, g, and S are matrices used to dimensionalize the stability and control 
derivatives and convert the state vector, z, to z’: 


g=l|u v wp q r|’, 


g = dynamic pressure, 


S = diag{—S, S, —S, Sb, Sc, Sb}, 


M' = diag{1/Vr,1/Vr,1/Vr,b/2Vr, ¢/2Vz, b/2Vr}, 
and 


z' = M's, 


M’' = diag{0, c/(2Vr), b/(2Vr), 0, 0, 0}. 


Equation 2.16 can now be further expanded using expressions of the forces 


and moments derived above. 
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d Pie = — Buypx 0 oh, 
Ht ey a 0 — 8 Ja (Fwe x 2 Jp? we) Bip a 
a ae 
| 0 a eae BN ’ (2.23) 
where 
Ae SAR AN 
pew |={ [So || mom 
nes OY: aC ngt, . BC nye | aC 
0 G5 { Cro + 28 FE en ao M's + 2 aca } (2.24) 
Notice that there is a state derivative term on the right hand side of Equa- 
tion 2.23 due to the second order terms in the Taylor series expansion of the aerody- 


namic forces and moments in Equation 2.24. By bringing it to the left hand side of 


Equation 2.24 and combining terms, we obtain: 


OR aa —Fwpx 0 4 
dt Bug = 0 —8J5}(Fwe x 8 Jp? wp) 


E B B 
mryraseeem’ || ce | +Mi*} | cl 4: 


0 
® Fprop aC 
0 bie + #/TGS(Cr, + SEA) (2.25) 
where: 
BR 0 m 0 
Bmr_|w 
ir =| 0 a and Mr =| 7 nam 
and 
¥=1s— Mz Tas (2.26) 


Equation 2.25 expresses the derivative of the first six states of the nonlinear model 


in matrix form. It is solved in the user defined MATLAB Fen block, state-deriv.m, 
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which is available for inspection in Appendix A. The physical parameters specific 
to Bluebird are stored in a MATLAB .m file, Bluebird-data.m, and are called from 
within the function, state-deriv.m . In order to change the vehicle being modeled, 
one simply changes the constants in Bluebird-data.m . 

Next, the Euler angles were added as the additional three states of the 
model. The time history of the Euler angles is defined in Equation 2.4 and written 


in compact form as: 


A = S(A)? wa, (2.27) 


where 


0 cos ® — sin ® 
0 sin®secO cos® secO 


The user defined MATLAB Fen block, eul.m, solves Equation 2.27, the details of 


S(A) = 





1 sin®tanO cos® tanO | 


which can be found in Appendix A. 


Finally, the inertial position of Bluebird can be computed as follows. 


TP a ug = BRB Ug. (2.28) 


The rotation matrix, 5R, is implemented in a MATLAB Fcn_ block, 
posb2i.m, in order to convert the vector ey to the vector ‘v,,. The vector ae 
is then integrated to obtain a time history of the position of the air vehicle in the 
local tangent plane. To increase fidelity of the simulation, a first order model of the 
four actuators, Selevator, Srudder, Saileron, throttle, 18 included with a time constant 


of 1/12. The resulting nonlinear dynamic model now has sixteen states which are 
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computed using Equations 2.25, 2.27,and 2.28; summarized here for clarity: 


T 
b 


T pos 


states = | u vwpgqre® OW Xpos Ypos Zpos 6.,,, Ons Cum, 


yy 
— | B, il Ba F T T 
aa eee ict oe |e 


Ge ug | —"wpx 0 in 
dt Bp —X 0 —? Ja (Bw x BJ gw) 


7 B BP 


0 
B 
| anor | See + £7 95(Cr, + SEA) ! \, (2.29) 
A = S(A)? wz, (2.30) 
Tp = ERP y,,. a5 


The SIMULINK diagram of the nonlinear model is shown in Figure 2.4. 
5. Trim and Linearization 

For linear controller design, the nonlinear equations above must be trimmed 
and linearized for a typical cruise flight condition for Bluebird. A SIMULINK tool is 
available which can be used to find equilibrium points of nonlinear dynamic models. 
The user specifies which states and control inputs are to remain fixed along with 
their stationary values and the trim routine searches for values of the state and input 
vector for which the derivative of the state vector equals zero. With the trim condition 
known, another SIMULINK tool perturbs the states around the specified trim point 
in order to find the rate of change of the states and control inputs (Jacobian). The 


resulting linear model is returned in state space format. Since Equation 2.28 can only 
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Figure 2.4: SIMULINK Nonlinear Sixteen State Dynamic Model of an Air 
Vehicle 


be trimmed for v,, = 0, not a typical flight condition, we will trim Equations 2.25 
and 2.27, and then include Equation 2.28 for linearization. 

We are interested in trimming the model in velocity. Hopefully, the deriva- 
tive of the position states will never equal zero in flight. Also, in trim, the control 
inputs are equal to the actuator positions. Therefore, actuator states are also removed 
from the nonlinear model. The nine state nonlinear model of Bluebird used for trim 
is shown in Figure 2.5. 


A typical cruise flight condition for Bluebird is given by: 


20 






Elevator 
Rudder 
Aileron 
Throttle 











d(x)/at = f(x,u) 
Equation 1.23 






d(Euler/dt = S(Euler)*w 
Equation 1.25 





Figure 2.5: SIMULINK Nonlinear Nine State Dynamic Model of an Air 
Vehicle 


e flight speed equal to 73 feet per second 
e flight path angle equal to zero 
e wings level attitude 


The nonlinear model depicted in Figure 2.4 was trimmed at this condition. 
The trim values of the nine states, u, v, w, p, g, 7, ©, O, and WV, and the four 
control inputs, 6., 6,, 63, and 6; were returned. While the sixteen state nonlinear 
model cannot be trimmed in position, it can be linearized at an arbitrary position. 
The origin of {J} is conveniently chosen. These values were then used to linearize 


the complete nonlinear model, including position and actuator states. The resulting 
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linear model of Bluebird and numerical values for the trim condition are included in 


the Appendix B. 
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Ill. THE LINEAR QUADRATIC REGULATOR 
DESIGN 


The previous chapter developed a nonlinear model of Bluebird, which was used 
to derive a linear model for a cruise flight condition. In this chapter, a linear dynamic 
controller is developed to provide trajectory tracking for the linear model. LQR 
methodology was selected to design the controller. Based on design requirements, an 
intuitive means of manipulating the LQR gains is presented. See (Ref. 9] for details. 
The following is a brief review of the properties of an LQR controller utilized in this 


design process. 


A. LQR OVERVIEW 


Consider the linear system 


£ 
=| 
y 


where z € AR", u € R™ and z € R?. 


(3.1) 


Hood 
Q 
8 
+4 
S 
sa 


Suppose C/ D, = 0 and D, is full column rank. Then, 
z?z=27ClCyz +u’ DI Du. 


Define a cost, J, as follows: 


i= / (27 z)dt = / (27 CT C,2 + ut DT Dyu)dt (3.2) 


and let Q = C?C,, and R = D] D,. Note: Q 20 and R > 0. 
Assume (C}, A) is observable and (A, B) is controllable. Consider Figure 3.1. 
The standard LQR problem is to find a controller, u = K(s)z, such that the feed 
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back system in Figure 3.1 is internally stable and J is minimized. It turns out, one 


such controller uses a constant gain, u = —Kz, where 


K =—R™B'Pp, (3.3) 
and P solves the Alegbraic Riccatti Equation: 
A’ P+ PA— PBR ™'B'P+Q=0. (3.4) 


Figure 3.1 shows the feedback interconnection of the plant G and the controller K. 


Here the inputs w can include commands and disturbances. 





Figure 3.1: Standard LQR feedback configuration. 


It turns out that the controller, AK, has guaranteed simultaneous phase and gain 
margins of no less than 60 degrees and 3dB, respectively (Ref. 12]. Furthermore, the 
controller has asymptotic properties which are exploited in the design process and 
are discussed next. 


Define the Hamiltonian matrix, H, as: 


Let T be given by: 
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then: 


I 0 
=1 & 
els 


where P solves the Riccatti equation, Equation 3.4. 


Note, 


A-—BR“BTP —BR“BTp 
=| vast 
. ur =| 0 —AT + PBR-'BT | 


Therefore the eigenvalues of H are the roots of the following polynomials: 


Clearly, the eigenvalues of the Hamiltonian consist of the eigenvalues of G .; and their 


: 
det(sI —H) = det(s] -T~'HT) = det(s] -A+BR™'B" P)det((sJ+A7—PBR™'B7) 


unstable reflections about the imaginary axis. Let R = pR,, R, > 0, then; 


sI—A (1/p)R,: BT 
det(sI-H) =|" 5 ( se 


It can be shown that the det(sJ — H) can be equivalently expresses as (Ref. 9]: 


det(sI—H) = —1"det(sI—A)det(—sI—A)det(I+(1/p)R]'B7 (—sI—A*)"'C] Ci(sI—A) *B) 





Let 
¢(s) = det(sI — A) 
and 
6(s) = Ci(sI — A)“*B. 
Then 


det(sI — H) = —1"¢(s)¢(—s)det(I + (1/p)Ry* 6(—s)6(s))). (3.5) 
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The SISO example will best demonstrate what occurs to the eigenvalues of G 4 


as p is varied from 0 to oo. Let 
4(s) = $(s)/9(s) 
where 1(s) are the zeros of 6(s). Then, 
det(sI — H) = —1"4(s)¢(—s)(I + (1/p)¥(s)¥(—s)/4(s)¢(—3)). 


It follows that the eigenvalues of H are the roots of, 


$(3)$(—s) + (1/)¥(s)(—3). (3.6) 


Consider the feedback system shown in Figure 3.2. This is standard configuration for 


SISO root locus analysis and its characteristic equation is: 


$(3)6(—s) + (1/p)¥(s)o(—3), 


exactly the same as Equation 3.6. Since we know that the stable eigenvalues of H 
are the eigenvalues of G .;, standard root locus techniques show that 


as p goes to O [Ref. 9]: 


® p eigenvalues of G ., go to the stable zeros of C,(sJ — A)~'B or the stable 
reflection of the unstable zeros of C(sI — A)~'B, where p is the number of 


zeros of C;(sf — A)“'B. 
e n-p eigenvalues of G .; go to -co in Butterworth patterns. 
as p goes to co 


e n eigenvalues of G . go to the stable eigenvalues of A and the stable reflections 


of the unstable eigenvalues of A. 
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Figure 3.2: Feedback Configuration for Root Locus Analysis. 


B. DESIGN REQUIREMENTS 


These properties of an LQR controller were utilized in a design process in order 


to meet the following design requirements. 


1. Zero Steady State Error 


e Achieve zero steady state values for all error variables in response to ramp 
commands in position along the x, y, and z inertial axes. Note that a ramp 
command in postion corresponds to a constant heading, constant velocity 


trajectory. 
2. Bandwidth Requirements 


e The input-output command response bandwidth (command-loop band- 
width) along any of the three command channels should be no greater 


than 1 radian per second and no less than 1/10 radian per second. 


e The control-loop bandwidth should not exceed 12 radians per second for 
the elevator and aileron actuators, and 5 radians per second for the throttle 


actuator. These numbers represent 80 % of the corresponding actuator 





A 


bandwidths and shall ensure the actuators are not driven beyond their 


linear operating range. 
3. Closed Loop Damping 


e The dominant closed loop eigenvalues should have a damping ratio of at 


least 0.7. 


C. THE SYNTHESIS MODEL 


The synthesis model is the primary interface between the control design 
and the LQR algorithm. At the heart of the synthesis model is a linear model 
of Bluebird developed in Chapter II. 


Bluebird has four control inputs, namely elevator, rudder, ailerons, and 
throttle. The elevator and throttle are natural choices for controlling x and 
Z position in steady state. The remaining two control inputs could be used 
to control the lateral variable (y position). Both rudder and aileron provide 
means of generating accelerations in the lateral plane. In fact, rudder is more 
effective at_ generating sideslip than aileron. In the linear plant, lateral position 
is the double integral of lateral acceleration. Subsequently, the resulting LQR 
controller will attempt to use rudder to null out errors in lateral position, 1.e., 
to turn the plane. However, the desired controller response is to bank to turn 
using ailerons and to use rudder for turn coordination. Furthermore, in the 
presence of wind, it is desired that Bluebird fly wings level, crabbed into the 
wind, rather than use a wing down, top rudder technique. For these reasons, 


the rudder was removed as a control input to the linear model. 


As can be seen from Table 3.1, the dutch roll mode of Bluebird is lightly 


damped. This light damping of 0.111 could pose a performance problem. Since 
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rudder is available and not used in the design of the trajectory controller, the 
nonlinear model of Bluebird was modified to include a yaw damper for improved 
dutch roll damping. Yaw rate was fed back to the rudder through a constant 
gain block with a value of 0.55. Additionally, the rudder was removed as an 
external input. Note that Bluebird is still fully controllable with the remaining 


three control inputs. 


TABLE 3.1: EIGENVALUES OF BLUEBIRD 


[Mode | Frequency | Damping | 
[Longitudinal | rad/sec |_| 
[Short Period [5.9 | 0.735 _| 
[ Phugoid | 0.497 | 0.0344_| 
[Lateral-Directional| | ——_—*d 
[Dutch Roll | 24 | Oa _| 
[Spal | 0038 [1 __| 
[Roll Response [4572 [1 | 










The nonlinear model of Bluebird with three inputs and integral yaw damper 


was linearized, as per Chapter II, returning the linear model, 


oe aa (3.7) 


= Ff 


where 
je 
ru vwpgrdO WV Xpos Ypos Zpos be, Sapos Stoos | 


and 


4% 
l= | bstewapnr Onctupat Sthrottle | 


This linear model was used in the LQR design. The eigenvalues of Bluebird 


with a yaw damper are given in Table 3.2 where it can be seen that the dutch 
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roll mode has been damped out. Note that the number of states decreases to 


fifteen since the rudder actuator state was removed. 


TABLE 3.2: EIGENVALUES OF BLUEBIRD WITH YAW DAMPER 


[Mode | Frequency | Damping | 
[_ Longitudinal | rad/sec |__| 
[Short Period | _5.9 | 0.735 _| 
[_ Phugoid | 0407 | 0.0344 _| 


[Lateral-Directional| | 
[Dutch Roll | 295 | 05 | 
[Spiral | 0.1788 | 1 | 
[ Roll Response | -4.5686 [1 | 





Consider Figure 3.3. Here K is the controller to be designed, G is the 
linear model of Bluebird, and the block, 5, within the dotted line is the synthesis 


model. 


The signal, w, represents the commanded trajectory inputs: 
i 
w= | X POSemd iL BOSeaG Z POS cmd D ond Onna ond ° 


The signal z, represents the linear and angular position states in the linear 
model. 


z, =| Xpos Ypos Zpos ® O v |" 


The signal e represents the errors between the commanded and current trajec- 
tory. The signal z is comprised of the outputs of the matrices, Q, and R. Since 
zero steady state error is desired while tracking a ramp command in inertial 


position, two integrators were placed on each error signal. This also ensures 
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Figure 3.3: Synthesis and Analysis Model 


perfect tracking of constant heading trajectories in the presence of a constant 
wind disturbance. Thus Q was chosen to be; 


n 0 0O];e @ & 


Q=|/0 m 0 era 
0 0 gq Smee 
The values of c,, were chosen to place six transmission zeros from u to z at 
appropriate locations. If they are well chosen, six poles in the closed loop plant 
will move very near to the placed transmisssion zeros. With that in mind, the 


transmission zeros were chosen as appropriate target locations for the poles 


added by the addition of the error states. 


The gzz weightings are used as a mechanism for obtaining the desired 
command bandwidth. Increasing the value of g,, increases the relative propor- 
tion of that error state in the regulated output vector z. The resultant LOR 
gain increases the command bandwidth in that channel in order to move the 


controlled state to its commanded value more quickly. 
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The matrix FR is a constant diagonal matrix required to be full rank. Since 


the plant G has three control inputs, f is of the form, 


T11 0 0 
n= 0 T9 0 
0 0 T33 


The elements of R are used as a mechanism for selecting the control bandwidth. 
An increase in rz, increases the relative proportion of that actuator energy in the 
regulated output z. The resultant LQR gain decreases the control bandwidth 


of that control input. 


D. THE DESIGN PROCESS 


Design requirements given in the previous section are SISO in nature. 
They are expressed as bandwidth limitations of the individual actuators and 
rise time and damping characteristics along the command channels. Note, the 
rise time is inversely proportional to the command bandwidth. The following 
LQR design process provided a means of obtaining a multivariable solution to 


achieve SISO design specifications. 


With an appropriate linear representation of Bluebird and a synthesis 
model that incorporated well placed transmission zeros, the design ” knobs” 
were adjusted in order to meet performance requirements. The design ” knobs” 
are the elemental weightings, gz, and r;z, in the Q and R matrices. The design 


process iterated through the following steps. 


1. Initially let g,, equal 1. Iteratively determine weights for R to sat- 
isfy control loop bandwidth requirements. Increasing r,, decreases the control 


bandwidth along that channel. 
2. With R from step 1, iteratively determine weights for gz, to satisfy 
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command loop bandwidth requirements. Increasing gz; increases the command 


bandwidth along that channel and decreases the rise time. 


3. If it is required to increase the damping of a lightly damped mode, use 
an eigenvector decomposition to determine the primary states affecting that 


mode. Include a weighting on the derivative of those states in the output z. 


4. Ensure that control-loop bandwidths are still satisfactory with the 
values of g,, in step 2. It is possible that all of the performance requirements 


are not acheivable within control bandwidth limitations. 


5. Connect the LQR controller to the linear plant and evaluate the per- 


formance in terms of command response and disturbance rejection. 
6. Confirm satisfaction of other design requirements, including damping. 


7. If any step is unsatisfactory, go back to the synthesis model and make 
appropriate changes. ‘Transmission zeros may need to be added, moved, or 


deleted. Synthesis model outputs may need to be reevaluated. 


Bode plots were used to determine compliance with the requirements. 
After five iterations through the seven step process, the following values for Q 


and R matrices resulted in a controller design that met design requirements. 


05) Omen] | eoze os 
qg=| 0 10]]1 % ode 
0 o1}{1 o% odes 
5000 0 0 | 
R=| 0 1000 0 
7 1000 | 


The transmission zeros created in the sythesis model are shown in Ta- 


ble 3.3. 


33 


TABLE 3.3: TRANSMISSION ZEROS OF SYNTHESIS MODEL 


| Channel | czi | coo | ¢es_| Fregency (rad/sec) | Damping | 
| Xpoo | 1 [0.2] 001 {| Ol | 1 | 
| Ypos_ | 1 [0.41 0.0625{ 0.25 | 0.8 
| poe | 1 {04} 0.0625{ 0.25 | 08 






E. LOR CONTROLLER PERFORMANCE 


The eigenvalues of the feedback interconnection of the plant and controller 
are given in Table 3.4. It is apparent that the zeros created in the synthesis 
model were well placed and attracted the integrators created by the addition 
of the error states. There are two sets of lightly damped poles. These do not 
present a problem because their frequency is an order of magnitude greater 
than the frequency of the eigenvalues associated with the trajectory commands. 
Notice that the actuator poles did not change, indicating that the control band- 
widths were slower than the actuator bandwidths. Actuator models provide a 
simple means of determining if the control bandwidths exceeded the actuator 


bandwidths. 


Figures 3.4 through 3.6 depict the control-loop bandwidths for the eleva- 
tor, aileron, and throttle. The cross coupling between longitudinal and lateral 


flight controls was so slight that it is not shown due to scale. 


Figures 3.7 through 3.9 depict the command-loop bandwidth for step com- 
mands in inertial position. Notice that there is some coupling between X com- 


mand and Z response; the rest are essentially uncoupled. 


A summary of the resulting command and control bandwidths achieved 


is presented in Table 3.5. 


TABLE 3.4: EIGENVALUES OF THE FEEDBACK SYSTEM 


[___Mode | Frequency (rad/sec) | Damping | 
[XAxisResponse| 0.08 «| ~—0.92_—| 
[Y Axis Response| 0.21 —~*4|_0.77_| 
[Z Axis Response | __ 0.21 —~*4|~_0.77_| 
[elevator [| 21 —~*i| 1.0 _| 
[aileron | —'23~—~S=~dY~SCi 
[throttle [| _24_—-+4| 10 _| 
[others | 062~~*«( 0.79 | 
2 a a a 
= a 
a 
dan. | 08 | 
| ee | 














elevator 
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oe a ee 
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Figure 3.4: Control-Loop Bandwidth: Elevator Channel 


The response to two types of trajectory commands is of interest. The first 
is the reponse to a ramp command in Y position. This corresponds to a change 
in the heading of the commanded trajectory. The response in terms of angle of 


bank and heading activity is shown in Figure 3.10. The controller achieves the 
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Figure 3.5: Control-Loop Bandwidth: Throttle Channel 
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Figure 3.6: Control-Loop Bandwidth: Aileron Channel 


desired result of turning Bluebird to the required heading. The nonminimum 


phase response of the heading state is due to adverse yaw. 


The response to a ramp command in Z position corresponds to the re- 


sponse to a change in flight path angle of the commanded trajectory. Figure 3.11 
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Figure 3.7: Command-Loop Bandwidth: X Position Channel 


Y channel 





10 
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Figure 3.8: Command-Loop Bandwidth: Y Position Channel 


shows the response to this command in terms of pitch angle, 0, and altitude z. 


Note that the positive z direction is down, hence the negative pitch angle. 


Finally, the response to a constant wind disturbance is shown in Fig- 


ure 3.12. The wind orientation is such that it affects all three axes of Bluebird 
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Z channel 
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Figure 3.9: Command-Loop Bandwidth: Z Position Channel 


TABLE 3.5: COMMAND AND CONTROL BANDWIDTHS 


[Loop | Break Frequency | 
[_ Control Loop | rad/sec | 
[Elevator [40 +I 
[_Aileron [| __5.0_| 
[Throttle [10 _| 
| Command Loop [| 
[ XAxe | ov | 
[YAxs [10 _| 
[Zaks [10 | 















equally. The wind disturbance begins at t = 0. 


38 


0.05 


0.04 


: Heading 


0.01 


Bank Angle 





-0.01 i i 
O 5S 10 15 20 25 30 
Time (secs) 


Figure 3.10: Bank Angle and Heading Response to Ramp in Y Command 
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Figure 3.11: Pitch Angle and Altitude Response to Ramp in Z Command 
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Figure 3.12: Trajectory Error Due to a Constant Wind Disturbance 


IV. CONTROLLER IMPLEMENTATION 
ON THE NONLINEAR PLANT 


) In Chapter III, a linear controller was designed to control the trajectory 
of an air vehicle. However, this controller cannot be implemented, as is, on 

: the nonlinear plant. It would only be effective for commanded trajectories that 
represent relatively small perturbations from the specific trajectory for which 

it was designed. Intuitively, it is clear that the dynamics of the plant do not 

depend on the heading angle V. Furthermore, the orientation of the force of 

_gravity is the only change in the dynamics of the air vehicle due to changes in 

® or O. It turns out that these issues were addressed in [Ref. 10], where a new 
methodology for implementing cantrollers on nonlinear plants is proposed. The 


method involves differentiating some of the inputs to the controller, hence the 


term, D-implementation. 


This chapter begins with a general description of the structure of D- 
implementation. Furthermore, the specifics of its implementation on the non- 


linear model in SIMULINK are discussed. Next, the fidelity of the nonlinear 


simulation is improved by incorporating output feedback. This step involves 





inclusion of high fidelty sensor models and Kalman filters. Finally, all of the 


pieces of the complete nonlinear simulation are brought together in SIMULINK. 





A. D-IMPLEMENTATION 


Using the development in Chapter II, the vehicle dynamics can be ex- 


pressed in state space form as follows: 


4] 


| 


d 
dt 2 Ug > fi (Coa WB, u) +7 R(A)g 


d 

FF Bin = fu( Pv,” WB, U) 
d 

at Peg =5 R(A)? veg 

d 


where f, and f., are continuously differentiable and u € R® denotes the vector 


of controliinputeseleoadenselthemovatienmeciie ine 
=| asl, 2 | 1, Ald | 204 |, 
Hr=[PQ]. w=[P 4) 
where z; € R®, rz. € R® and L € R®**. Furthermore, we define 
ris | i | € RP, (4.2) 


as the vector of linear and angular position commands that Bluebird must 


track. With this notation, the dynamics of the augmented plant can be written 


as follows: 
Fy = fi(zv,u) + fo(zp) 
Ldn 
c Qe = y2—-?Tr 
=) ey = [0 Mu—r) “a 
i — h(zy, u) 
Y2 = yp 


where y; and y2 are the available measurements, e; and e2 are the trajectory 
errors separated into linear and angular components. Notice that L is only a 
function of the orientation vector A = [0 J|z,. For simplicity of exposition, we 


have not included any extra dynamics for the actuators or sensors. 
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The set € of trajectories where the plant G is expected to operate is given 


by 


vw = 
W 
——— ris Lng, tho; To) : —_ = a TO € ee 


fu( ups Bpostto) + fa(po) = 0 


where I corresponds to the set of prescribed linear and angular positions of 
Bluebird. This is a broad definition of trimmed flight. Notice that it does 
not preclude the presence of an inertial acceleration due to centripetal force. 
As usual, we restrict the angular positions to some subset of [—2/2,2/2] x 
(—72/2,2/2) x [—2/2, 2/2], as the inverse of L(.) is not defined at 6 = 7/2. 
Notice, from the definition of z,, € € and equation (4.1) it follows that: 


Buy € E 3? wep = constant 


AE€ E> A = constant. 


Notice that the set € is easily parameterized by zp, = ro € I. Given 


(Lup) Zpp, Uo, To) € E, we obtain 


Y1o -= (2, Uo) 
on: ce, 
o = [I O](y2, — 70) := 0 


é2, := [0 I](y2, —7o) := 0.. 


C1 


Let 5z,, d2,, 5u,dy1, Sy2, dr, de; and Se, correspond to small per- 
turbations of z,, Zp, U, ¥1, Y2, 7, €1, and e2 about the nominal values 
Zin) Lpoy UO; Yio> Y2, TO; C19; and eg, respectively. The family of lin- 


earized models associated with the rigid body G and the set € is defined as 
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Gi := {Gi (ro), ro € T'}, where 


6ty = Aj6z,+ Adz, + Bou 

Sz, = Léz,+ Ag6zp 

6y, = C62, + Didu 

Gi (ro) :=4 dy. = dz, (4.4) 

be, = [J 0](dy2 oa dr) 

Sep = [0 I](dy2— 6dr) 

a = U( Ze; TO, Uo), 

and 
0g ae R(Ao)S" (Ao) 0 —) R(Ao)Vo a ae (Ao) 
Aa= | 9 0 | a | 0 0 | 


(4.5) 
Matrices Az and A, were derived using the identity in Appendix C. The intent of 
this derivation is to isolate the plant dynamics that are a function of Ao. Notice 
that A» represents the contribution of the force of gravity to the dynamics of 
Bluebird and A, represents the sensitivity of Bluebird’s trajectory to changes 


in the air vehicle’s spatial orientation. 


Let ro € E be given. Define 


bzy => A)62y a A26z» - Bébu 
dzp = b2y+ As6zp 
6y, = Cdr, + D,du 
Gi. (ro) = § by2 = Szy (4.6) 
Se; = [I O](dy2 — 6r) 
6€2 = (0 I] (6y2 = dr) 
a = Viren Td, Uo), 
where 
4 _ 0 g xP R(Ao) 7 0 —vp9X 
A, = | 0 0 ; A= 0 0 : (4.7) 


Notice that Equation 4.6 decribes the linear model of Bluebird used for 
the design of the LQR controller in the previous chapter, where ro was chosen 
as the origin of {I} with Ag equal to zero. Note, at this condition, 7? R(0) = I. 


Recall, the structure of the controller developed in Chapter III is given by: 
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621 = B26 2.2 
Ki =4 622 = [de 0]7 (4.8) 
du = C1621 a C262 62 + Dady1 le [Des D2] [dex be)’. 


Based on K; , we propose to implement the following controller for the 


nonlinear plant G : 


Zc Bel '(A)[e, 0]* 
Leo Cate + Cl (A)fer 0]? + Days 
an) = +Deal7(A)[0 é2]7 (4.9) 
[0 T\y2 
L 2 =F DsL7* (A)[ex oie. 


u 

Figure 4.1 shows the block diagram of K (A). Notice that A serves as a 
gain scheduling variable. As a function of A, L~! serves to properly resolve 
the trajectory error at the input to the controller. Note, the controller forms 
the derivative of the measured outputs, y;. Recall, y; is the measurement of 
the states z, and the dynamics of z, are essentially independent of the air 
vehicle’s spatial orientation or position in {J}. An integrator at the output of 
the controller serves to recover the properties of the linear design. The error 1s 
formed using the outputs y2 and the commanded trajectory r. Recall, y2 is the 
measurement of the states z,. L~' serves to resolve this error, originally formed 


in {I}, in {B}. 


It turns out that the implementation of Figure 4.1 has an important prop- 


erty discussed next. First we need to make the following assumptions: 
Al. Dim(2,2) = dim(u) = dim(y2). 


A2. The matrix 


sl —Ag Boo 
“~Yel Ce2 


has full rank at s = 0. 
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c= 0 0 Be | 
a Cer C2 Dea [Des D2) 


Figure 4.1: Block diagram of the nonlinear controller K (A) 


A3. The matrix pair (A.},C.) is observable. 


Also denote by T(G,, Ki) : dr — dy2 the closed loop linear system that 
results from connecting G), to K; , and by T(G,, Ky )(s) its transfer matrix. 
Similarly, we let 7:(G , K )(ro) denote the linearization of the closed loop system 
T(G , K ) at the equilibrium point determined by ro and let T(G , K )(ro)(s) 


be its transfer matrix. Then the following hold. 


e the feedback systems 7;(G , K )(ro) and T(G, , Ki ) have the same closed 


loop eigenvalues; 


Ti(G ,K )(ro)(s)= — L(Ao)T (Gio » Kr )(s)L~*(Ao); 
Ao = (0 ic 
Thus, the eigenvalues of the linearizations at each operating point are 


preserved; furthermore, the input-output behavior of the linearized operators 
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is preserved in a well-defined sense. The reader is referred to [Ref. 10] for a 
complete discussion on approximations to this method that avoid using pure 


differentiation. The proof of this result is contained in Appendix C. 


B. D-IMPLEMENTATION OF THE CONTROLLER IN SIMULINK 


In general, three steps are required to implement the linear controller 
developed Chapter III on the nonlinear plant. First, the controller needs to 
form the derivative of z, as per Equation 4.9. Recall, z, is equal to the vec- 
tor [Pucg, Bw]. The first three elements of the vector, By... are available as 
processed acceleration outputs from the IMU. Therefore, the controller need 
not compute the derivative of ?u,, since it will have it available directly. The 


derivative of ?wg is computed by the controller. 


Secondly, the controller needs to act on the vectors, e, and e2. The linear 
position error e; is formed as the difference in commanded and current posi- 
tion. The vector e; is then multiplied by the transformation matrix PR. This 
effectively resolves the linear trajectory error in the body-fixed reference frame. 
Along this position trajectory, there exists a corresponding trajectory of Euler 
angles. Recall that z, = constant is one of the constraints on the set of tra- 
jectories. This includes a broad range of flight conditions such as steady turns, 
steady pull-ups, climbing or descending turns, or constant heading. While it 
is natural to define the linear trajectory as a sequence of positions, it is more 
convenient to define the derivatives of the Euler angles rather than the values of 
the angles directly. Consider, for example, that for many trajectories of interest 
in €, the components of A can be described by: ® = 0; © = 0; and W = desired 
turn rate. Furthermore, the relationship between the rates of change of the 


Euler angles and ?wg is used as a means of resolving the angular position error 
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in {B}. Recall, 


Bug = S(A)A. (4.10) 


Therefore, the derivative of the Euler angle states is formed and the commanded 
Euler angle rate is removed to form €2. This error is resolved in {B} using 
Equation 4.10. The integrator at the end of the controller recovers the effect of 
the initial differentiation. 


Thirdly, the required error states are formed by integrating the rotated 
linear trajectory error vector e;. Figure 4.1 indicates that integral action 1s 
accomplished at the output of the controller in order to recover the original 
properties of the linear design. This accounts for one of the integration steps 
on the error. Therefore, only one additional integrator is required to provide 


double integration action on the trajectory error e}. 


Figure 4.2 shows the D-implemented controller in SIMULINK, file plant1.m. 
See Appendix B for a complete description. The LQR gain has been parsed into 


several separate matrices for clarity of control action. 


C. GENERATION OF THE TRAJECTORY COMMANDS 


The commanded trajectory is specified with respect to the inertial refer- 
ence frame. At this point, it is assumed that a knowledge of the air vehicle’s 
performance capabilities is known and that the specified trajectory is within 
those capabilities provided there is no wind. The air vehicle has certain airspeed 
restrictions with respect to the air mass that cannot be violated regardless of 
the desired trajectory to be tracked. These restrictions typically provide lower 


and upper bounds on the velocity of the air vehicle with respect to the airmass. 
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Figure 4.2: D-Implementation of controller on Bluebird 


An example of a lower limit is the stall speed of the air vehicle. Such limits are 
usually based on fundamental physical limitations of the airframe and a ”fly- 
able” trajectory can become ”unflyable” under certain conditions. Therefore, 
a logic block positioned between the commanded trajectory and the controller 
ensures that the commanded trajectory can be flown at current flight conditions 
within user defined indicated airspeed limits, shown in Figure 4.3. The com- 
manded linear trajectory enters the block as a time stamped position fix in the 
inertial reference frame. Onboard sensors provide both inertial velocities from 


the IMU and air mass velocities from the pitot-static system. Furthermore, a 
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dual vane device instrumented on Bluebird provides both a and § measure- 


ments. Note: a close approximation to these readings could be obtained from 


IMU measurements as outlined in Chapter II. 


Poaition 
du/h 
Schedule 
indicated Airspeed 
Proceesed 
eee 
Schedule 
tower 
= [e 


Figure 4.3: Commanded Trajectory Logic Block 


With these measurements, the wind vector resolved in {J} is calculated as: 


‘W =) Roo ph om (4.11) 


where 


and V, is the indicated airspeed obtained from the pitot-static system. 


The commanded indicated airspeed of the air vehicle, Vi-ema, is calculated as 


Vizond = VUand a W|, 


00 


where ‘vung is the numeric derivative of the commanded linear trajectory. If the 
commanded indicated airspeed is not within specified limits, the commanded 


trajectory is altered as follows. The angles # and w are calculated as follows: 


9 = tan“'(v,/vz) (4.12) 


and 
p = sin” (v,/|"vemal) (4.13) 


where the components of /vgnq are [vz, vy, Vz]?. Note that the angles 9 and w 


define the commanded velocity vector’s orientation in {J}. 


Finally, the amount that V;_.ng is outside of indicated airspeed limits is 
subtracted from the magnitude of /v,,4, resulting in the magnitude of the new 


commanded inertial velocity, termed V. V is then resolved in {J} according to: 


cos()cos(w) —cos(@)sin(y) —sin(@) 
T= sin(p) cos(p) 0 V (4.14) 
sin(9)cos() —sin(A)sin(y)  cos(A) 


where /vmoq is the new commanded inertial velocity. This command is inte- 
grated and sent to the controller as the commanded trajectory. The MATLAB 


.m file that implements this logic is windlogic.m and can be found in Appendix 


A. 


The net effect of the trajectory logic block is simple. When Bluebird runs 
up against one of its airspeed limits, the commanded trajectory can no longer be 
followed. A choice is made to maintain the direction of the commanded velocity 
but change its magnitude. Notice that this method does not affect the turn rate 


associated with the trajectory and subsequently, no processing of the angular 
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trajectory commands is required. In this way, Bluebird is never commanded to 


fly a trajectory that would force it to exceed the performance limits. 


The performance of the trajectory logic block can be seen in Figure 4.4. 
The lower limit for Bluebird was arbitrarily choosen as 63 feet per second. Blue- 
bird is initially flying due north at a ground speed of 73 feet per second, crabbed 
into 20 feet per second of wind from the west. The commanded trajectory turns 
90 degrees to the east. Notice that the original trajectory would result in an 
indicated airspeed of 53 fps while the revised trajectory results in a commanded 


trajectory of 63 fps. 
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Figure 4.4: Example of Commanded Trajectory Revision 


If the commanded trajectory is generated using a velocity rather than 
position schedule, the differentiation block in Figure 4.4 can be removed. A 
velocity schedule is specified in {J} as a sine wave of appropriate magnitude, 
frequency, and phase along the x and y axis. The commanded ground speed 
corresponds to the magnitude and the commanded turn rate corresponds to 


the frequency of the sine function. Constant heading flight is a subset of these 
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trajectories where the frequency is zero. This method of generating trajectory 
commands makes determining turning rate (Aang) simple. As an example, con- 
sider the case of generating the velocity schedule for a circular flight pattern 
at a ground speed of 100 fps and a turn rate of 0.1 radians per second. The 


derivative of the commanded trajectory, r, parameterized by time is: 


z azis velocity = 100stn(0.1t + pi/2) 
y azts velocity = 100sin(0.1t + 0) 
z aris velocity = desired climb or descent rate 


Pim | 5 -_ (4.15) 
6 = 0 
- = 01 


The SIMULINK block that generates the commanded trajectory is shown 


in Figure 4.5. 
Velocity Angular 
Schedule Linear 
Processed 


Commands 
Indicated Airspeed 
Limit 
Inertial Velocity 
Logic 


Figure 4.5: Generation of Trajectory Commands 


53 


D. STATE FEEDBACK TO OUTPUT FEEDBACK 


Up until this point, the development of the tracking controller has dealt 
exclusively with full state feedback. This section will detail a method whereby 
the full state feedback is replaced by high fidelity models of the onboard sensors 
in conjunction with Kalman filters designed to provide optimal state estimates, 


using onboard sensor data. 
1. Sensor Modeling 


This work builds upon sensor models developed in two prior theses. 
In [Ref. 1], a detailed model of the inertial measurement unit, IMU, is devel- 
oped. In [Ref. 2], a detailed model of the GPS unit is developed. The IMU is 
a compact, lightweight, low power unit which integrates nine sensor measure- 
ments in one box. These sensors are three axis accelerometers, three axis rate 
gyros, pitch and roll inclinometers, and a magnetometer. The accelerometers 
are instrument grade, signal conditioned, and temperature compenstated. Full 
scale output is t/ — 3 g’s. The accelerometer’s frequency response is flat past 
100 Hz. However, the antialiasing inside the IMU limits the effective bandwidth 
of all of the sensors to 20 Hz. An internal initialization program allows the unit 
to compenstate for accelerometer bias and cross axis error. Table 4.1 shows the 


specifications of the accelerometers incorporated into the sensor model [Ref. 8]. 


The rate gyros used by the IMU are solid state vibrating element 
angular rate sensors. This relatively new technology uses no moving parts. A 
piezoelectric bender element is mounted end to end but rotated at a 90 degree 
angle. The element fastened to the base is resonantly driven such that the 
sense element swings a reciprocating arc. Under zero angular rate conditions, 


the motion of the sense element due to the drive element does not produce any 
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TABLE 4.1: ACCELEROMETER CHARACTERISTICS 


[Acceleration Bandwidth | ___20Hz___ 
0.5% of Full Seale 






bending of the sense element. When a rate of rotation exists, Coriolis forces 
cause momentum to be transfered into the plane perpendicular to the motion 
of the drive element, thus causing bending of the sense element. A pressure 
transducer picks up a signal from the sense element when it is bent that is 
proportional to the angular rate with a phase dependent on the direction of 
rotation. Figure 4.6 shows the configuration of two rate sensors mounted in a 
*tuning fork” configuration. Table 4.2 shows specifications of the rate sensors 


incorporated into the sensor model [Ref. 8]. 





Figure 4.6: Angular Rate Sensor 


90 





TABLE 4.2: ANGULAR RATE SENSOR CHARACTERISTICS 


| Rotational Rate Range +114.6deg/sec_ | | 
| Rotational Rate Bandwidth | 20Hz ~~ | : 
Rotational Rate Bias 2.0% of Full Scale : 


| Rotational Rate Scale Factor | 0.5% of Full Scale | 
| Rotational Rate Noise Floor | 0.05% of Full Scale | 
[Gross Axis Sensitivity | 0.8% of Full Scale | 





The inclinometers utilize a liquid crystal pendulous sensor. It is a 
low bandwidth sensor ( approximately 0.12 radians per second) that is meant 
to be integrated with the rate sensors for high bandwidth measurements of 
angular position. The fluxgate magnetometer provides heading measurements. 


The specifications incorporated into the Euler angle sensor models are shown 


in Table 4.3 [Ref. 8]. 


TABLE 4.3; UNCLINOMETER AND MAGNETOMETER CHARAC- 
TERISTICS 


| Pitch and Roll Range | +50 deg | 
) Pitch and Roll Bandwidth | 1/2 Hz | 
| Pitch and Roll Accuracy | 0.2deg | 


[Heading Range | £180 deg | 
[Heading Accuracy | 3.0deg | 
[| Heading Repeatability | 0.5 deg _| 
[Heading Linearity | 05% _| 





GPS provides data in a form that can be converted to local tangent 
plane position. A brief summary of the errors included in the GPS model 


follows. 


GPS Error Sources 


e Selective Availability 
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— intentional degradation of pseudorange signal by Departmant of Defense 


e Clock Differences 


— drift and bias in GPS clock 


e Ephemeris Error 


— error introduced in converting pseudoranges to inertial position fix 


Kach of these sensor components is simulated in block diagram form in SIMULINK 
utilizing internal modeling principles based on manufacturer specifications and 
known sources of error. The upper level SIMULINK diagram of these sensor 
models is shown in Figure 4.7 and contained in the SIMULINK file plant1.m. 
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Figure 4.7: Sensor Models in SIMULINK 


of 


2. Kalman Filtering 


With the outputs of the modeled sensors available, Kalman filters 
were developed along the linear and angular position channels to provide optimal 
estimates of the states for the controller. An approach analogous to the LQR 


design was used. Consider the linear system described by: 


Ar + Bu+ Gw 
Cret+u 


(4.16) 


z 


where v and w are zero mean white noise with respective power spectral densities 


of V and W. 


A gain matrix, L, was found such that the Kalman filter given by: 


zg = Ar+ Bu+L(z—Cz) (4.17) 


produces an optimal estimate of z. The Kalman gain L is calculated as follows: 


L=YCTV-, 


where Y is positive semidefinite and solves the algebraic Riccati equation: 


AY + YA? -~YC'V-'!CY + GWG =0 


A synthesis model was formed that included the dynamics of the 
original plant. The IMU used in Bluebird incorporates an initialization routine 
that removes steady state bias from the sensors. Therefore, extra dynamics were 
not required in the Kalman filter to compensate for bias. The design process was 
primarily driven by the bandwidth limitations of the inclinometers and GPS. 


The values of V and W were used as ”knobs” in the iterative design process. 
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For the GPS sensor, a break frequency of 1/2 radian per second was desired. 
For the Euler angle sensors, a break frequency of 1/10 radian per second was 
desired. It was also desired to wash out the accelerometers and rate sensor 


biases at low frequencies. 


The Kalman filter for the position estimate blends processed ac- 
celerometer outputs with the GPS position fixes. Figure 4.8 shows the frequency 
response along the x axis of the Kalman position filter. The other two axes have 


similar dynamics. 
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Figure 4.8: Frequency Response of Position Filter 


The Kalman filter for the Euler angles blends the outputs of the angu- 
lar rate sensors, converted to Euler angle rates, with Euler angle measurements 
from the inclinometers and magnetometer. Figure 4.9 shows the frequency re- 


sponse of one channel of the Euler angle filter. The remaining two angle channels 


og 


are similar. 
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Figure 4.9: Frequency Response of Euler Angle Filter 


E. INTEGRATION OF THE FULL NONLINEAR SIMULATION 


The full nonlinear simulation can now be pieced together. Recall in Chap- 
ter II, the nonlinear rigid body dynamics were implemented in a SIMULINK 
block labeled Equations of Motion. If the simulation is expanded to include the 
effects of a moving airmass, the dynamics of Bluebird can be simulated at an 
arbitrary flight condition. Wind is usually referenced with respect to the iner- 
tial reference frame, therefore a SIMULINK block, Wind, is included in the full 
simulation whose output is a vector w, comprised of the wind velocity resolved 
in {I}. Next, the wind velocity is resolved in {B} and added to the inertial 


velocity of Bluebird, 7 Veg. The result is the velocity of Bluebird with respect 
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| 


| 





to the airmass, resolved in {B}. This velocity, rather than ?v,,, is used when 
computing the aerodynamic contribution to the total external force and moment 
used in Equation 2.25. A more detailed description of how this is accomplished 


in the .m file state_deriv.m is contained in Appendix A. 


All sixteen states are sent to a SIMULINK block, Sensors, that models 
the avionic sensor suite onboard Bluebird. The output from these sensors is 
appropriately processed in a SIMULINK block, Kalman filters. The filtered 
output is directed to the D-Implemented Controller block. The commands to 
the controller come from a trajectory block which uses the measured outputs 
from the filter block to process the commanded trajectory. The controller gen- 
erates actuator commands necessary to maintain the air vehicle on the com- 
manded trajectory, thus completing the loop. The complete top level view of 
the SIMULINK nonlinear simulation is shown in Figure 4.10 and contained in 
the SIMULINK file plant1.m. 
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Figure 4.10: SIMULINK Diagram of the Full Nonlinear Simulation 
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V. APPLICATION TO THE CONTROL OF 
BLUEBIRD 


A. D-IMPLEMENTED CONTROLLER PERFORMANCE CHAR- 
ACTERISTICS 


The performance characteristics of the trajectory tracking controller de- 
signed in Chapters III and IV were evaluated using a two step process. First, the 
sensor and filter blocks were removed and the controller was connected directly 
to the nonlinear equations of motion block. Utilizing pure state feedback, Blue- 
bird was flown along two fundamentally different types of trajectories. These 
two trajectories served as general examples of the set of all trajectories defined 
in Chapter IV, Equation 4.4. Next, the sensor and filter blocks were added. 
A general deparure and arrival trajectory, which is a combination of the two 
types of trajectories tested in the first step, was commanded and flown with 
the controller utilizing output feedback. Data from this simulation were used 


as input to a virtual prototype simulation discussed later. 


The dynamic flight simulations were started using the same initial condi- 
tion. At this initial condition, Bluebird is aligned with the positive x-axis and 
trimmed for level flight at 73 fps. The positive x direction is considered to be 
heading north. The mechanics of the dynamic simulation use a right-hand or- 
thogonal coordinate system described in Chapter II. As such, the positive y-axis 
is pointing east and the positive z-axis is pointing down. This choice is con- 
venient from a computational standpoint since it coincides with the body-fixed 


reference frame of Bluebird at the initial condition specified above. Typically, 
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however, the positive z axis is considered to be pointing up. If the right-hand 
orthogonality is maintained, the positive y-axis would then point towards the 
west. For ease of visualization, this is the cooridinate system used to display 


the results of the simulations. 


The simulations are intended to evaluate the capabilities of the controller 
in terms of the nature of trajectories that can be followed in a stationary air mass 
and in an air mass moving at constant velocity. Two basic kinds of trimmed 
flight serve as the bases for the test trajectories. Each test trajectory is flown in 


no-wind conditions, and then again with the wind added at some point during 


the flight. 


The simplest form of trimmed flight is constant velocity, constant heading. 
This corresponds to a trajectory defined by a ramp command in inertial posi- 
tion. This was the basic trajectory that the controller was designed to track. 
Figure 5.1 shows a three dimensional plot of the first test trajectory. In this 
case the trajectory encompasses 30 seconds of flight heading north at 73 fps 
followed by a 90 degree turn to join a trajectory heading east at 73 fps while 
climbing at 300 feet per minute. On the second flight, a wind of 20 feet per 
second from the north is added at the time the turn is commanded (elapsed 


time = 30 seconds). 


Figure 5.2 contains the first four graphs of flight data. The first graph 
shows the time history of Bluebird’s distance from the commanded trajectory. 
The next three graphs show the time history of the Euler angles. Consider the 
baseline flight (no-wind data). Bluebird begins the turn at an elapsed time of 
30 seconds and exits the turn at an elapsed time of 45 seconds. Approximately 


30 seconds later the trajectory error is nearly zero. In the presence of 20 feet 
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Figure 5.1: Test Trajectory #1 


per second of wind from the north, the trajectory error also goes to zero in 
about the same period of time. The graphs of the Euler angles indicate that 
Bluebird is flying wings level, crabbed into the crosswind, which is the desired 
result. Figure 5.3 shows the groundspeed and indicated airspeed during the 
flights. Notice that in both cases, the commanded groundspeed of 73 feet per 
second is eventually maintained. Finally, Figure 5.4 shows the time history of 


the control activity during the flights. 


Trimmed flight does not necessarily have ?wg equal to zero. For instance, 


any steady turning manuever fits the definition of trimmed flight given in Chap- 
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Figure 5.2: Trajectory #1: Position Error and Euler Angles 


ter IV. Figure 5.5 shows the three dimensional plot of the second test trajectory. 


In this case the test trajectory is a helix flown at 73 feet per second. The turn 


rate is one revolution per minute and the helix angle is 4 degrees which corre- 
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Figure 5.3: Trajectory #1: Velocity Data 
sponds to a climb rate for Bluebird of 300 feet per minute. For this test, no 
indicated airspeed limits were placed on Bluebird. 


Consider Figure 5.6 which shows the position error and Euler angle time 
history for the helix trajectory. Notice that with no wind the controller manuev- 


ers Bluebird to join the commanded trajectory with zero error in steady state. 





The constant pitch and bank angles confirm the steady state performance. On 
the second flight, a wind of 20 feet per second from the east was added at the 
start of the helical trajectory (elapsed time equal to 40 seconds). Intuitively, 


it is clear that the bank and pitch angle must vary as Bluebird traverses the 
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Figure 5.4: Trajectory #1: Control Activity 


helix. To an observer on Bluebird, the wind, while constant in {J}, represents 


a sinusoidal disturbance. This explains the sinusoidal nature of the position er- 


ror around the helix. Figure 5.7 shows the groundspeed and indicated airspeed 
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Figure 5.5: Test Trajectory #2 


around the helix during the two flights. For the no-wind flight, the commanded 
groundspeed is maintained while for the flight in wind, a one foot per second 
oscillation is experienced. Finally, Figure 5.8 shows the time history of the 
control activity during the flights. 


Four additional flights were flown using the helix trajectory in an attempt 
to ascertain the sensitivity of the position error to changes in commanded turn 
rate and wind velocity. Figure 5.9 shows the position error around the helix 
for three different turn rates with a constant wind of 10 feet per second. The 


dashed line corresponds to a turn rate of 3 degrees per second or 2 minutes 
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Figure 5.6: Trajectory #2: Position Error and Euler Angles 


per revolution; the solid line corresponds to a turn rate of 6 degrees per second 


or 1 minute per revolution; and the dash dot line corresponds to a turn rate 


of 9 degrees per second or 40 seconds per revolution. The error increases with 
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Figure 5.7: Trajectory #2: Velocity Data 


increasing turn rate. The 9 degree per second turn rate corresponds to a steady 
state angle of bank of thirty degrees, no wind. It may not be desireable to 
command trajectories requiring more than a certain angle of bank and this may 


place an upper bound on the error. 


Figure 5.10 shows the position error around the helix for three different 
wind velocities at a constant turn rate of 6 degrees per second. The wind varies 
in velocity from 10 to 25 feet per second. The local maxima values of the 


position error are proportional to the magnitude of the disturbance. 
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Figure 5.8: Trajectory #2: Control Activity 


B. AN AIRPORT DEPARTURE AND ARRIVAL FLIGHT SIM- 


“ULATION 


In many cases. the trimmed flight condition of an air vehicle changes often. 


A good example of this would be a standard instrument departure or arrival 
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Figure 5.9: Trajectory #2: Position Error for Varying Turn Rates 
to an airfield. These trajectories are typically a combination of constant radius 
turns and wings level flight, while often climbing and descending. ‘Trajectory 


position errors become critical when the air vehicle is on final approach with a 


constant heading, constant velocity trajectory. 


Consider Figure 5.11. If an airfield is imagined to be located at the origin, 





| then this trajectory would be indicative of a typical departure followed by a 
typical arrival to that airfield. The scenario utilizes turning trajectories of three 
different radii connected by straight line trajectories. The commanded velocity 


is a constant 73 feet per second throughout. Wind is initially zero. Thirty 
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Figure 5.10: Trajectory #2: Position Error for Varying Wind Velocities 


seconds into the flight, the wind is added at 10 feet per second from the east. 
At 90 seconds into the flight the wind is increased to 20 feet per second from the 
east. Finally, with Bluebird on final approach tracking a 4 degree glideslope, 
the wind is rapidly shifted 90 degrees to the north and decreased in magnitude 


to 5 feet per second. 


Figure 5.12 shows the time history of the position error, wind velocity, and 
Euler angles during the flight. Figure 5.13 shows the time history of the control 
activity. Note, however, the relative difficulty of analyzing data of this nature 


as itis somewhat difficult to visualize. Flight simulation data was saved to a file 
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Figure 5.11: Departure and Arrival at an Airfield 


and processed for compatiblity as an input file for a 3-D visualiztion software 
package, Designer’s Workbench. A virtual prototype of Monterey Airport and 
Bluebird was developed in {Ref. 13]. The simulation was then run as a departure 
and arrival to the virtual prototype airfield. In Designer’s Workbench, the flight 
can be viewed from multiple perspectives and virtual prototypes of standard 


cockpit displays further enhance visualization. One possible result is captured 


on video tape, {Ref. 14]. 
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Figure 5.12: Airfield Scenario: Position Error and Euler Angles 
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Figure 5.13: Airfield Scenario: Control Activity 
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VI. CONCLUSIONS AND 
RECOMMENDATIONS 


A. CONCLUSIONS 


Based on the data presented in this thesis, the following conclusions are 


drawn. 


e SIMULINK provides an effective environment for the developement of non- 
linear simulations for air vehicles. As a result of this development, a linear 
model of the plant at an arbitrary trim condition is easily obtained for 


design purposes. 


e LQR design techniques utilizing a synthesis model and weighting ” knobs” 
provide a straight forward means of obtaining satisfactory controller gains 


for MIMO systems while meeting design requirements. 


e D-Implementation of the linear trajectory tracking controller allows the 
controller to operate effectively on the nonlinear plant. In no-wind flight 
conditions, trajectories defined by an arbitrary [vo, wo] are tracked perfectly 
in steady state. For flight conditions with wind, rejection of a constant 
wind disturbance is accomplished along the family of trajectories defined 
by an arbitrary [vo,wo = 0]. However, for turning flight, a constant wind 
disturbance in {J} is seen as a sinusoidal disturbance in {B} and a sinu- 
soidal tracking error results. For moderate bank angles and turn rates, the 


errors are usually small. 
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e Preprocessing of the trajectory commands by an adaptive filter allows for 
steady state control of the air vehicle’s velocity with respect to the air mass 
in the forward path, thus not affecting stability. With sufficient margins for 
transient deviations in indicated airspeed, this would allieviate the major 
concern of stalling the air vehicle when tracking an inertial trajectory in 
wind. 

e When analyzing a nonlinear plant and controller, test simulations are vital 
and in some cases the only means of performance evaluation. The three 
dimensional plots and time history graphs are fine for simple trajectories, 
but are difficult to analyze for more complex cases. The capabilities of a 
virtual prototyping software package, like Designer’s Workbench, are im- 
pressive. The enhanced situational awareness and visualization capabilities 
of watching the designed controller operate on a virtual prototype allow 


for a ”pilot’s perspective” feedback not otherwise attainable on the desk 


top. 


B. RECOMMENDATIONS 


Based on the conclusions presented above and the experience of developing 
the simulation package presented in this thesis, the following recommendations 


are made. 


e While the rigid body equations of motion are nonlinear with respect to 
the kinematics involved, they are completely linear with respect to the 
stability and control derivatives. The constant coefficient stability and 
control derivatives could be replaced by functions when further flight data 


is available. 
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e A similar design process used for the trajectory controller could be done 


using #(,. design methodology. 


e The trajectory preprocessor could be used to convert an inertial fixed tra- 
jectory into an air mass fixed trajectory. This might have applications 
where the air vehicle’s inertial position is of secondary importance com- 


pared to its performance with respect to the air mass. 


e Running simulations real time in Desiner’s Workbench rather than using 
batch post processed data would be the next logical step. Further work 
might lead to virtual prototype visualizations based on real-time simula- 


tions or downlinks from actual air vehicles. 
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APPENDIX A: MATLAB FILES 


The SIMULINK models of Bluebird (plant1.m & plant2.m) use the fol- 


lowing MATLAB .m files as user defined functions. 


STATE.DERIV.M 


Mhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhyhhhhhhhhhhhthtt 


h 


Function to calculate derivative of u,v,wW,p,q,r 4, 
based on h 
1: kinematics 4 

2: gravity A 

3: propulsion yA 

4: aerodynamics h 

h 

Variables brought from workspace: h 


x = [contrl inputs, state variables(1 - 9), wind vel] 4% 


(da,de,dr,dtrt,u,v,w,p q,r,phi,theta psi, wind xyz)% 


h 

Variables called from function "blue_data" h 
/ 

rho = air density h 

b = wing span yA 
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y/ c = wing cord 

yi s = wing area 

% Cfo = Steady state force term 

4, Cfu = Stability derivative for control inputs 
4, m = airplane mass 

i Ib = inertia tensor matrix (body frame) 
yA To = Thrust scale term 

y J Pe = Engine postion matrix 

4 

| 

y; 


WALnhAbbhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhrhhhhhhhhhhielbtlelh bh 


function accel = state_deriv(x) 


444%%% Function call to get the aircraft data 


(u0 ,wO,rho,Cfx,Cfo,Cfu,Cfxdot,s,b,c,m,Pe,To,Ib] = blue_data; 


Lhhhhh seperate the combined vector into seperate elements 


u = [x(1); x(2); m@ool; 

dtrt = x(4); 

state = [x(5); x(6); x(7); x(8); x(9); x(10)]; 
lambda = [x(11); x(12); x(13)]; 
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wind = [x(14); x(15); x(16)]; 


“444% calculate velocity wrt the airmass and form state vector 


“A444 that will be used to calculate the aerodynamic forces/moments 


lias = u + wind 


statei = [ias(1)-u0; ias(2); ias(3)-w0; x(8); x(9); x(10)]; 


WALLY =«calculate total velocity, vt 


vt = (ias(1)°2 + ias(2)°2 + ias(3)°2)~.5; 


WALL = calculate qbar 


qbar = .5*rho*(vt“2) ; 


YUULLY «calculate Mi 


Mi = diag([i/vt, i/vt, 1/vt, (.5*b)/vt, (.5%c)/vt, (.5*b)/vt]); 


Ahhihh calculate M2 


M2 = diag([0, 0, (.5*c)/(vt72), 0, 0, 01); 


YUULLY «= calculate Sprime 
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Sprime = diag([-1, in =; bi, Cc, b]*s); 


YYuUUYY, calculate Mu 


Mu = inv([eye(3) *m,zeros(3) ;zeros(3) ,Ib]); 


YUU, calculate Tw2b 


alpha = ias(3)/vt; 


beta = ias(2)/vt; 


Ti = [cos(alpha), 0, -sin(alpha); 0,1,0; sin(alpha), 0, cos(alpha)]; 


T2 


[cos(beta), -sin(beta), 0; sin(beta), cos(beta), 0; 0,0,1]; 


Tw2b = (T1i*T2, zeros(3); zeros(3), T1*T2]; 


YUU, «= calculate Chi 


Chi = eye(6) - Mu*Tw2b*qbar*Sprime*Cfxdot*M2; 


W44N4% calculate Propulsion matrix 


Prop = [ eye(3); 


0,-Pe(3) ,Pe(2); 


Pes); Of Patt) ; 


-Pe(2) ,Pe(1) ,0]; 


hhhhhh calculate gravity vector and rotation matrix {I} to {B} 
Rot = (1, 0, -sin(lambda(2)); 

0, cos(lambda(1)), cos(lambda(2))*sin(lambda(1)); 

0, -sin(lambda(1)), cos(lambda(2))*cos(lambda(1))]; 


Ru2b = [Rot;zeros(3)]; 


g = [0; 0; 32.174]; 


FgU = m*g; 


hhhhh, put the components due to gravity; thrust; and control surface 


WAL = deflections together for their contribution to x-dot 
thrust = Prop*To*dtrt; 

gravity = Ru2b*FguU; 

ctrl = qbar*(Tw2b*(Sprime*(Cfo + (Cfu*u)))); 

xdotu = (Mu*(ctrit+tthrustt+gravity) ) ; 


Ahhhhth calculate kinematic contribution 


omegax = [0,-state(6) ,state(5) ;state(6) ,0,-state(4) ;-state(5) ,state(4) ,0]; 


wxIb = (-inv(Ib)) *(omegax*Ib) ; 


Rot = [-omegax, zeros(3); zeros(3), wxIb]; 


xdotrot = Rot*state; 


hhhhh state vector feedback component xdot 


xdotcfx = qbar*(Mu* (Tw2b* (Sprime*(Cfx* (Mi*state1))))); 


Aihhhh add three components of xdot together and premult by inv(Chi) 


xdot= inv(Chi)*(xdotrot+xdotcfx+xdotu) ; 


WALLY, «= return xdot 


accel=xdot; 


EULER_B2I.M 


Mhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhbhhhhttolteltlhthttehtete tt tht ttt te 


: ’ 
h Transformation [p q r] to lambda-dot y 
yA ys 


TALMAIMAMAA MLM LI LILIAN IIIA LMLI AIIM IMAI AI IAMAMAIAMAMN 
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function ldot = eul_b2u(x) 


“WAL, «seperate the composite vector ’x’ into [p q r] 


wU44% and [phi theta psi]. 
omega = [x(1); x(2); x(3)]; 
phi=x(4) ; 

theta= x(5); 


psi=x(6) ; 


44777 calculate the rotation matrix {I} to {B} 


44%%% based on euler angles 


Rb2u = [1, sin(phi)*tan(theta), cos(phi)*tan(theta) ; 


0, cos(phi), -sin(phi); 


0, sin(phi)*sec(theta), cos(phi)*sec(theta)] ; 


UUULY, calculate lamda-dot 


ldot = Rb2u*omega ; 


EKULER12B.M 


Mhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhbhihhhhbhyhhhhhhhhhhhhhhbhhhhehhhtt 


87 


h 
%, Transformation lambda-dot to [pq r] 
yA 


YUMUIMAMSUMSMAMAMAUSISMAMLMLMALIAMAMAMMSMAMAM MMMM ISMN 


function omegadot = eul_i2b(x) 


“thhh seperate the composite vector ’x’ into lambda-dot 


“444% jj and [phi theta psi]. 


1dot = [x(1); x(2); x(3)]; 
phi=x(4) ; 
theta= x(5); 


psi=x(6) ; 


y by ly Ay calculate the rotation matrix {B} to {I} 


LAKE based on euler angles 


Rb2i = [1, sin(phi)*tan(theta), cos(phi)*tan(theta) ; 


0, cos(phi), -sin(phi); 


0, sin(phi)*sec(theta), cos(phi)*sec(theta)] ; 


yyy yA calculate lamda-dot 


omegadot = inv(Rb2i)*ldot; 
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POSITION.B2I.M 


Mhhbhhhbhhhphbhhhhbhrhryhhhbhhbhhhhhbhbrhrhhhbhhhhyhhbhhrohhbhhhhhyhhhhhhhhthtl 


h y/ 
%, From the workspace: yA 
y/ y/ 
%, 1: free vector ’u’ resolved in {B} e(1:3) yA 
%, 2: euler angle vector {phi,theta,psi} e(4:6) yA 
h h 
7, Returns: | 
y/ y/ 
y/ 1: free vector ’u resolved in {I} y/ 
y/ h 


hhhhhhhhhhhhhhbhbhphhhbhhhrhryhyhhhhhhhhrrbhhhhbhbhrhhhhhhhhhhhhhhhhhh 


function ans = pos_b2i(e) 


hhhh, this will rotate the trajectory error through phi, theta, psi 


LEAN, «=6 from {b} to {i}. (3-2-1 transformation 


phi=e(4) ; 
theta=e(5); 


psize(6) ; 
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m_psi = [cos(psi),sin(psi) ,0 
-sin(psi) ,cos(psi) ,0 


070-1) 

m_theta = [cos(theta) ,0,-sin(theta) 
0,1,0 

sin(theta) ,0,cos(theta)]; 

m_phi = [1,0,0 

0,cos(phi) ,sin(phi) 

0,-sin(phi) ,cos(phi)]; 

rotb2i = inv(m_phi*m_theta*m_psi) ; 


u = [e(1); e(2); e(3)]; 


ans = rotb21i*u; 


POSITION 12B.M 


TUYMAMAIULMLMLNLI ANAT IIIA IIIA III IIIA AIAIII AIAN 


y ’ 
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y/ From the workspace: 


h 2: euler angle vector {phi,theta,psi} e(4:6) 


4, 1: free vector ’u’ resolved in {I} e(1:3) 
yi 

4, Returns: 

yA 

4, 1: free vector ’u resolved in {B} 


Mhhhhhhhhhhbhhhhhhhhhhhhhhbhrhhhhhhbhhyihhhhbhhhhhhyhhbbhhhhhhhhhhhhh 


function ans = pos_i2b(e) 


Aiuh% this will rotate through phi, theta, psi 


VIL = «from {i} to {b}. (3-2-1 transformation) 


phi=e(4) ; 
theta=e(5); 


psi=e(6) ; 


m_psi = [cos(psi) ,sin(psi) ,0 
-sin(psi) ,cos(psi) ,0 


0,0,1]; 


m.theta = [cos(theta) ,0,-sin(theta) 


0,470 
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sin(theta) ,0,cos(theta)]; 


m_phi = [1,0,0 


0,cos(phi) ,sin(phi) 


0,-sin(phi) ,cos(phi)]; 


roti2b = (m_phi*m_theta*m_psi) ; 


u = [e(1); e(2); e(3)]; 


ans = roti2b*u; 


LIMIT _LOGIC.M 


TUAUMAMAI ALISA ILI AI AMA UAMAIAI AAAI AAAI IAIAI ALUMINA SNL 


yA 

yf funtion to limit trajectory commands, if required 
yA 

y from workspace: 
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ue 


Ze 


commanded inertial velocity 


inertial wind 


h 
yf 


ys 3: lower IAS limit yi 


h 4: upper IAS limit y/ 
ys h 
%, returns: revised commanded velocity h 
h yA 


MWhhbhhhhhbhhhhhbhhhbhbhhhhhhhhhrhrhhhbhhhhhhubhbhhhhhhhhhhhhhyhhhbhhhhth 


function vcom=limit (u) 


W444 seperate u 


vel_i = [u(1);u(2);u(3)]; 


wind_i = [u(4),u(5) ,u(6)]; 


11=u(7) ; 


ul=u(8) ; 


Ahhh calculate magnitud and direction of commanded velocity 


gs=sqrt (vel_i(1)°2 + vel_i(2)°2); 


ang=atan2(vel_i(2),vel_i(1)); 


hhhhhh calculate commanded IAS (steady state) 
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vt= sqrt ((vel_i(1)+wind_i(1))°2 +(vel_i(2)+wind_i(2))°2 +(vel_i(3)+wind_: 


Lhtehiete Prepare return variable (may not be limited) 


vcom = vel_i; 


y by yy Check limits and revise if outside 


lfsvt > nde 


vcom(1) = (gs - over)*cos(ang) ; 
vcom(2) = (gs - over) *sin(ang) ; 
end; 

If Vis 21: 


under = ll - vt; 


vcom(1) = (gs + under) *cos(ang) ; 


vcom(2) = (gs + under) *sin(ang) ; 


end; 


BLUEBIRD_DATA.M 


Whhittelhttototeh tt tt tetetetototetotoh tote tetotetototeloletetetetetotote lolol toto to lototeteteto ole te tote te totale le 


¥ 3 
yA Aircraft data for Bluebird yA 
4, 4, 


Lhhhhhhlhhhttelelthhttettotelteh tte telteltetetetetoltettetetoleltetete tole tetetoteltetetelelh tote ete 


function [u0,w0,rho,Cfx,Cfo,Cfu,Cfxdot,s,b,c,m,Pe,To,Ib] = blue_data 


“inh, trimmed flight speed and angle of attack 


u0 = 73.3; 


wO = QO; 


AAAhL% Density: Sea level- std day 


rho = .0023769; 


yy Ay by by derivative matrix due to state variables 
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Ahhh rows: [CD CY CL Cl Cm Cn] 
hhhhh col: [u v/U w/U pq rl] 


Cfx = [0 0 .188 00 0; 
0 -.31 0 0 0 .0973; 
0 0 4.22 0 3.94 0; 
0 -.0597 0 -.363 0 .1; 
0 0 -1.163 0 -11.77 0; 


0 .0487 0 -.0481 0 -.0452]; 


AALS derivative matrix due to control inputs 


Ahhhh rows: [CD CY CL Cl Cm Cn] 
y by by by by bi col: [elev rud ail] 


Cfu = [.065 0 0; 
QO .0697 0; 

472 .0147 0; 

QO .0028 .265; 
~1.41 0 0; 


0 -.0329 -.0347]; 


Ahhhh derivative matrix due to x-dot (alpha_dot & Beta_dot) 
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Cfxdot = [00000 0; 
00000 0; 
001.3200 0; 
00000 0; 


000000); 
Ahhh steady state force vector 
Gio = [.03- 0; .385;: 0; 0: Ol]; 


Ahhh physical dim. 
hhhhh WT =55 LBS. 


m= 1.7095; 


22.38; 


om 
fl 


12.42; 


a 
i) 


1.802; 


hhhhh engine data (4 HP motor) 


Pe = [0; 0; 0]; 
To = [15 ;0;0]; 
y Ay by by Ay inertia tensor matrix 


Ib = [ 10 0 0; 0 16.12 0; 0 0 7.97]; 
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APPENDIX B: SIMULINK FILES 


The nine state nonlinear model of bluebird is contained in the SIMULINK 
file FOM_9.m and was trimmed at a flight condition of 


e flight speed equal to 73 fps 


e flight path angle equal to zero 


using the TRIM command. The resulting trim values for the state vector and 


input vector are: 


73.3 
0 

—0.0023 0 

0 

ci 0 | andu= 0 
0 

0 0.2858 

0 
0 


The LINMOD command was used to linearize the sixteen state nonlinear 
model of Bluebird (contained in the SIMULINK file EKOM_16.m and described 
in Chapter III) about this trim point. The resulting linear system is contained 


in the MATLAB file Linear16. mat. 


The rudder was removed as a control input (remove the second column 
of the B matrix) and the resulting linear model was used as a basis for the 
synthesis model contained in the SIMULINK file synthesis.m. This synthesis 


model was used to determine the LQR gain. The synthesis model, Q and R 
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weighting matrices, and resulting LQR gain is contained in the MATLAB file 
LOR_dat.mat. 


The full nonlinear simulation is contained in the SIMULINK file, plant1.m. 
The MATLAB .m file stmdata loads the workspace with the appropriate vari- 
ables. The file stmdata calls the .m file trajectory.m in order to generate the 
trajectory schedule. Any changes to the commanded trajectory or wind distur- 


bance schedule can be made in trajectory.m. 


A version of the nonlinear simulation that does not use the filter and 
sensor blocks is contained in the SIMULINK file plant2.m. It runs considerably 


quicker than plant1.m. 


99 


APPENDIX C: D-IMPLEMENTATION 


PROOFS REFERENCED 


Identity. Let x € R® = const be given. Then 
d iy I -1 
7a (Bit(A)r) = —pR(A)z x S~'(A). 


and 


a(? R(A)z) = z x? R(A)S7*(A). 


Proof: To derive both equations we will need Poisson’s Law: 


© (bR(A)) =" we x} R(A), 


and the following identity: 


axb=-bxa 
for any vectors a and 6 of compatible dimensions. Now, consider 
a (BA)z) = (s(pR(A))2 +5 R(A) Ts 
=" wy x, R(A)z =—ER(A)z x? wa, 
using equation (C.3), (C.4) and z = const. 
Next, by the chain rule we get 
dy Gad; d 
qetAls) = a R(A)z)aA 


d B 
= (5 R(A)z2)S(A) Pp. 


Equation (C.1) now follows by comparing equations (C.5) and (C.6). 
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(C.1) 


(C.2) 


(C.3) 


(C.4) 


(C.5) 


(C.6) 


To obtain equation (C.2), consider 
© (bR(A)PR(A)) = 5 (GR(A))PR(A) +4 R(A)S(PR(A)) = 0, (C.7) 


since £.R(A)? R(A) =I VA. Now, using equations (C.3) and (C.7) we get: 


d 


(FP R(A)) = -PR(A)Pup x. -) 


Finally, following the steps in the derivation of equation (C.1) we obtain: 


+ (PR(A)z) =x x R(A)S~1(A). 


Theorem ..1 Suppose that assumptions Al through A3 hold. 
re Vig Dim(z-2) = dim(u) = dim(y2). 


A2. The matriz 
| sl—A, Bey | 


“~el C2 


has full rank at s = 0. 
A3. The matriz pair (Aja,C) ts observable. 
Then for each equilibrium point of G in E the following properties are 


observed: 


e the feedback systems T;(G , K )(ro) and T(G,, , Ki ) have the same closed 


loop etgenvalues; 


Ti(G ,K )(ro)(s)= — L(Ao)T (Gig , Ki )(s)L~"(Ao); 
Ao = (0 I]z,, 
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Proof: In the proof we set the controller matrices D.,, Dez D-3 to zero. This 
does not change the results but considerably simplifies the algebra. Further- 
more, we will drop explicit dependence of the controller parameters on a. Let 
Ca nh | ,Uo, To € E) be given. Consider the feedback interconnection 
of the linear plant G,, (Ao) and linear controller K; . The state matrix F of this 
feedback system has the following form: 


Ay A, BoC BaC 2 


I Ag 0 0 
Fe= : 
Gh a Bs (ext) 
C. 0 0 0 


Next we linearize the feedback interconnection of the plant G and the controller 
K . However, in order to that, first we must determine the values of the con- 
troller states z., and 2,2 along the trajectory ro € €. From equation (4.9) we 


obtain: 


d d 2 
di” = ete ct, aE Be dt? "5 Bol *(Ao)eo 


d 
qr = Care, + CaL~*(Ao)eo 
Uo = Lec2- 


Notice, since along ro: 
€éo = 0, y1, =const, 2.9, = Uo = const 
we get 


7 2elhp = Aa Lelo 


dt 


0 ad Calcio ° 
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Now, using Assumption A3 we conclude that z,,, = 0. 


In order to compute the linearization of the feedback interconnection of 
G andK , we must first obtain the linearizations of equations (4.3) and (4.9) 
about the operating points (z,,Zy,,Uo) € € and (Za, = 0,22, = Uo, Yip = 
0,€9 = yo, —To = 0) respectively, determined by ro € €. The linearization of 
the plant G is given by (4.4). The linearization of the controller K has the 


following form: 


éa = Aata + Babs + BesL-*(62 — p) 
ba = Cab + Coa L~* (82 — p) 
n = c2. (C.10) 


It is easy to verify that the state matrix M of 7)(G , C )(ro) is 


A, Ag 0 B 
= L Ag 0 0 
M=) BiQ\A1 BaCyAz+ Bal" Aa BaC,B oo 
0 Cal Co 0 
Let 
I 000 
0 L200 
i 00] 90 
0007 
Obviously, 
I 0 00 
<1 « 0 LZ 0 0 
ie = 0 0 J0 
0 0 OT 


Now using simple algebra it is easy to show that 


M = P-'!MP 
Ay Ao 0 B 
I Ag 0 0 
BaCiA, BaCiA2+ Bs Aa BaC,B 
0 Cre2 Cen 0 
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The proof of the first part of the theorem now follows from Assumption A2 and 
an observation that the matrices F and M are in the form of the matrices F 
and M. The proof of the second part of the theorem consists of the following 


steps: 


1. compute the transfer matrix of the linearization of the controller K (A) 
using equation (C.10) from controller inputs 6,, 92, p to controller output 
7; 


2. apply a similarity transformation 


I 0 
A= 4 mae 


to the linearization of the plant G (= G; (ro)) given by equation (4.4) and 
derive the transfer matrix from the control inputs of the linear plant 7 to 
the outputs @, and 6, using this new state-space realization; 

3 compute the feedback interconnection of the transfer matrices obtained in 


steps 1 and 2 to get the final result. 


A simple computation shows that the transfer matrix from the controller inputs 


6,, 92, p to the controller output 7 is given by: 


7(s) Cer (sl — An), (eala. 


+ Bardi(s) + CoL-'=(62(s) — As) 
6,(s) 
“ | 62(s) | | (C.12) 
L(s) J} 
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K;(s) 





where Kj(s) is the transfer matrix for the controller K; . 


Applying similarity transformation P, to equation (4.4) and computing 


the transfer matrix from 7 to 0; and 63 results in: 


(C.13) 





where the transfer matrix is given in packed matrix form. A simple observation 


shows that 


| om | = P>*Gra(3)i(s) 


where G;,»,(s) is the transfer matrix for the plant @,, . 


Now routine algebra shows that the transfer matrix from p to 92 of the 
feedback interconnection of the transfer matrices in equations (C.12) and (C.13) 


is given by: 


Ti(G ,K )(ro)(s) = L(Ao)T (Gig » Ki )(s)L~" (Ao). 
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